#pragma once

#include <rclcpp/rclcpp.hpp>
#include <manipulation/msg/mani_process_cmd.hpp>

class ManiCmdPublisher {
public:
    explicit ManiCmdPublisher(const rclcpp::Node::SharedPtr& node)
    : node_(node) {
        cmd_pub_ = node_->create_publisher<manipulation::msg::ManiProcessCmd>("mani_process", 10);
        while (!cmd_pub_->get_subscription_count()) {
            RCLCPP_INFO(node_->get_logger(), "Waiting for ROS1 subscribers to mani_process topic (Do you open ros1_bridge?)...");
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
        RCLCPP_INFO(node_->get_logger(), "ROS1 subscribers connected to mani_process topic.");
    }

    void head_up_little() {
        RCLCPP_INFO(node_->get_logger(), "Publishing head_up_little command to mani_process topic.");
        manipulation::msg::ManiProcessCmd cmd;
        cmd.level1_name = "task0";
        cmd.level2_name = "head_up_little";
        cmd_pub_->publish(cmd);
    }

    void head_down_little() {
        RCLCPP_INFO(node_->get_logger(), "Publishing head_down_little command to mani_process topic.");
        manipulation::msg::ManiProcessCmd cmd;
        cmd.level1_name = "task0";
        cmd.level2_name = "head_down_little";
        cmd_pub_->publish(cmd);
    }

    void head_origin() {
        RCLCPP_INFO(node_->get_logger(), "Publishing head_origin command to mani_process topic.");
        manipulation::msg::ManiProcessCmd cmd;
        cmd.level1_name = "task0";
        cmd.level2_name = "head_origin";
        cmd_pub_->publish(cmd);
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Publisher<manipulation::msg::ManiProcessCmd>::SharedPtr cmd_pub_;

};
